package de.hhn.mim.psb;

import java.util.ArrayList;
import java.util.Random;


/* 
 * TODO: Version ohne TRE Berechnung und ohne calcTrafo
 * 
 */

/**
 * Klasse mit Beispielen zur Transformation von Koordinaten,
 * FRE und TRE Berechnung und Hauptkomponentenanalyse
 * 
 * beisp1_EigenwertUndVektorBerechnung();  
 *   Zeigt wie man Eigenwerte und Vektoren berechnen kann
 * beisp2_PCAScriptMaierHein();   
 *   Eigenwerte nach einem Beisp. aus Script Maier-Hein  
 *   (03a, Segmentierung Teil1, Bachelor-Schwerpunkt, Diagnose-/Therapiesyst.)    
 * beisp3_TREmitPolarisDaten();   
 *   Berechnung mit 3 Landmarken aus einer Polaris-Navigation 
 * beisp4_TREBerechnung()
 *   Berechnung der Transformation, FRE und TRE mit 5 Landmarken
 *     
 * @author Rolf Bendl
 * 
 * 2012-05-12: Beispiele erstellt
 * 2014-02-05: 
 * beisp4_TREBerechnung
 * Statt wie bisher Math.rand wird ein Objekt der Klasse Random verwendet,
 * um bei jedem Aufruf die gleichen Zufallszahlen und Koordinaten zu bekommen
 *
 * an calcTRE wurden die Zielkoordinaten (fixed p1a-p5a) uebergeben
 * geaendert, es werden nun die floating Koordinaten (p1b-p5b) uebergeben
 * Eigentlich egal, letztendlich ist es abhaengig davon, wie die Koordinaten
 * in calcTRE interpretiert und weiterverarbeitet werden. Wenn (wie jetzt)
 * die floating Koordinaten uebergeben werden, sollten diese in calcTRE
 * in das Zielkoordinatensystem transformiert werden. Da die transformierten
 * Koordinaten nicht exakt mit den Zielkoordinaten uebereinstimmen,ergibt sich
 * jeweils geringfuegig unterschiedliche Hauptachsentransformation und ein
 * geringfuegig unterschiedlicher TRE. Wenn die transformierten Koordinaten
 * nahe bei den Zielkoordinaten liegen, sollte der Unterschied aber unerheblich
 * sein. Letztendlich k�nnen bei einer interaktiven Definition der Landmarken 
 * ebenso Fehler bei der Bestimmung der Zielkoordinaten wie bei der Bestimmung
 * der floating-Koordinaten passiert sein. Wichtig fuer die Bestimmung des TRE
 * ist die relative Lage des Zielpunktes zu den Landmarken. Der TRE ist klein
 * wenn er in der Naehe oder im Zentrum des Koordinatensystems liegt, das durch 
 * die Hauptachsentransformation bestimmt wird. Je weiter er davon entfernt 
 * ist, desto groesser wird er.
 *
 */
public class LMRegistSamples
 {
  /**
   * es werden keine Parameter beruecksichtigt
   * @param args
   */
  public static void main(String[] args)
   {
    LMRegistSamples  lms = new LMRegistSamples();
    
    Vect3D v1 = new Vect3D(0, 0, 0);
    Vect3D v2 = new Vect3D(100, 0, 0);
    Vect3D v3 = new Vect3D(0, 100, 0);
    
    lms.beisp1_EigenwertUndVektorBerechnung(); 
    lms.beisp2_PCAScriptMaierHein();   // Eigenwerte Beisp. Script Maier-Hein      
    lms.beisp3_TREmitPolarisDaten();   // PolarisWerte (3 Landmarken)

    lms.beisp4_TREBerechnung();
   }

  /**
   * 
   */
  public LMRegistSamples()
   {
   }

  /**
   * Beispiel 1:
   * Demonstriert das Vorgehen bei der Hauptkomponentenanalyse
   * 
   * erzeugt 11 Landmarken, groesste Verteilung in y, dann in x, dann in z
   * Rotation um x = 30�  
   * PCA sollte eine Basis liefern,welche die rotierten Daten wieder in ihre 
   * urspruengliche Orientierung bringt.
   * 
   * Eigenwert und Vektorberechnung mit transformierten Punkten
   * Ergebnis: 
   * eigenVects (Rotation 0�)         eigenVects (Rotation 30�)   
   *  0.000     1.000     0.000       -0.000    1.000    0.000
   *  0.000     0.000     1.000        0.500    0.000    0.866
   *  1.000     0.000     0.000       -0.866    0.000    0.500
   * eigenValMat (Rot 30�)
   * 18.182     0.000     0.000
   *  0.000    90.909     0.000
   *  0.000     0.000  3272.727
   *  
   * Wenn die Werte nicht rotiert werden:
   *  groesster Eigenwert fuer Vektor (transp)[0,1,0] => y, zweiter [1,0,0] => x
   *  
   * Interpretation der Eigenvektoren als Basis
   * Transformation der transformierten Punkte mit transponierte (inversen)
   * Eigenvektor-Matrix 
   * => urspruenglich y-Werte stehen in z-Elementen (3.Position), 
   *    x-Werte in y (2. Pos), z-Werte stehen an 1. Position 
   * 
   * Da die Hauptachsen auf den Schwerpunkt bezogen sind, muss von den 
   * transformierten Werten der Schwerpunkt abgezogen werden.
   * 
   */
  public void beisp1_EigenwertUndVektorBerechnung()
   {
    System.out.println("beisp1_EigenwertUndVektorBerechnung\n");
    
    Vect3D [] points = new Vect3D [11];
    Vect3D [] tpoints = new Vect3D[11];
    Vect3D [] result  = new Vect3D[11];
    points[0] = new Vect3D(128,   8, 10);
    points[1] = new Vect3D(128,  68, 10);
    points[2] = new Vect3D(128, 128, 10);
    points[3] = new Vect3D(128, 188, 10);
    points[4] = new Vect3D(128, 248, 10);    
    points[5] = new Vect3D(128, 128,  0);
    points[6] = new Vect3D(128, 128, 20);
    points[7] = new Vect3D(118, 128, 10);
    points[8] = new Vect3D(138, 128, 10);
    points[9] = new Vect3D(108, 128, 10);
    points[10] = new Vect3D(148, 128, 10);

    Matrix3D rx    = Matrix3D.rotationX(Math.toRadians(30));
    Matrix3D trafo = new Matrix3D(1);
    
    trafo = trafo.mult(rx);    
    System.out.println("Original Punkte: ");
    for(int i = 0; i < points.length; i++)
      points[i].print("point[" + i + "]");
    System.out.println();
    System.out.println("Transformierte Punkte (Rotation 30� - x-Achse:");
    System.out.println("[");
    for(int i = 0; i < points.length; i++)
     {
      tpoints[i] = trafo.mult(points[i]);
      //tpoints[i].print("tpoint[" + i + "]");
      System.out.println(tpoints[i].x+" "+tpoints[i].y+" "+tpoints[i].z+";");
     }
    System.out.println("];");
    
    PCA3D pca = new PCA3D(tpoints);
    
    Vect3D center = pca.calcCenterOfMass(tpoints);
    center.print("Zentrum (transformierte Punkte)");
    double [][] eigenVectors   = pca.getEigenVectors();
    double []   eigenValues    = pca.getEigenValues();
    double [][] sortEigenVects = pca.getSortEigenVects();
    double []   sortEigenVals  = pca.getSortEigenVals();
    
    Matrix3D eigenMat = new Matrix3D(eigenVectors);
    Matrix3D sortEigenMat = new Matrix3D(sortEigenVects);
    rx.print("rx");
    eigenMat.print("EigenMat");
    Vect3D myEigenVals = new Vect3D(eigenValues[0], eigenValues[1], eigenValues[2]);
    myEigenVals.print(    "eigenValues    ");
    Vect3D mySortEigenVals = new Vect3D(sortEigenVals[0], sortEigenVals[1], sortEigenVals[2]);
    mySortEigenVals.print("sortEigenValues");
    sortEigenMat.print("sortEigenMat");
        
    // Trasformiere in neues Basis-System 
    // Die Richtung mit der groessten Varianz wird auf die x-Achse abgebildet 
    Matrix3D tpEigen = sortEigenMat.transpose();
//  tpEigen.print("transponierte EigenMat");
    
    for(int i = 0; i < points.length; i++)
     {
      result[i] = tpEigen.mult(tpoints[i]);
      result[i].print("result[" + i + "]");
     }
    Vect3D tcenter = tpEigen.mult(center);
    tcenter.print("Center");
    for(int i = 0; i < points.length; i++)
     {
      result[i] = result[i].subtractVect(tcenter);
      result[i].print("result[" + i + "]");
     }    
    System.out.println("\n\n\n");
   }
  /**
   * Test der PCA mit Daten aus Script Dr. Maier-Hein
   * 03a_Segmentierung_Teil1.pdf (Schwerpunkt Bachelor, SS 2012)
   * Vorgabe Datenmatrix
   * | 3 5 7 9 |
   * | 2 3 4 5 |
   * | 2 2 2 2 |
   * Ergebnis sollte sein:
   * EVals = [0, 0, >0]
   * v1 = [0,0,1], v2 = [-0,45,0.89,0] v3 = [-0.89,-0.45,0]
   * Ergebnis in PCA.calcEigenvalueDecompostion():
   * EVals = [0, 0, 6.25]
   * v1 = [0.447, -0.894], v2 = [0,0,1], v3 = [0,894, 0.447, 0]
   * Im Prinzip das Gleiche, allerdings mit umgekehrten Vorzeichen
   * Wenn in PCA.calcEigenvalueDecomposition SingularValueDecomposition
   * statt EigenvalueDecomposition verwendet wird, enthaelt die svd_umat die 
   * gleichen Werte wie im Beispiel von MH, die Vektoren sind dann aber bereits 
   * nach dem groessten Eigenwert sortiert d.h. v3 und v1 haben dann Position 
   * gewechselt
   */
  public void beisp2_PCAScriptMaierHein()
   {
    Vect3D [] points = new Vect3D [4];
    points[0] = new Vect3D(3,2,2);
    points[1] = new Vect3D(5,3,2);
    points[2] = new Vect3D(7,4,2);
    points[3] = new Vect3D(9,5,2);
    
    System.out.println("beisp2_PCAScriptMaierHein\n");
    PCA3D pca = new PCA3D(points);
    double [] EVals = pca.getEigenValues();
    double [][] eigenVectors   = pca.getEigenVectors();
    double [][] sortEigenVects = pca.getSortEigenVects();
    double []   sortEigenVals  = pca.getSortEigenVals();
    
    Matrix3D eigenMat     = new Matrix3D(eigenVectors);
    Matrix3D sortEigenMat = new Matrix3D(sortEigenVects);
    
    Vect3D myEigenVals = new Vect3D(EVals[0], EVals[1], EVals[2]);
    myEigenVals.print(    "eigenValues    ");
    eigenMat.print("eigenVectors ");
    System.out.println("\n\n\n");
    
   }
  /**
   * Berechnet TRE fuer 3 Paare mit vorgegebenen Landmarken
   * Testdaten aus der Polaris Navigation
   * 
   * Die notwendige Transformation wird mit LMRegist.calcTrafos(lms)
   * berechnet. 
   * Diese Methode berechnet Transformationen, die sich aus allen moeglichen
   * LM-Kombinationen und Reihenfolgen ergeben. Dazu benutzt sie eine weitere
   * Form von calcTrafo, welche nach Berechnung der Transformation
   * die transformierten Punkte mit den Zielkoordinaten vergleicht,
   * wenn ein interner Schalter (checkForExactCorrespondencies auf true
   * gesetzt wird.
   * Da es sich in diesem Beispiel um Koordinaten handelt, die in einem
   * Navigationsversch ermittelt worden sind, passen in keinem Fall alle
   * Landmarken exakt aufeinander.
   * 
   */
  public void beisp3_TREmitPolarisDaten()
   {
    System.out.println("beisp3_TREmitPolarisDaten\n");
    
    System.out.println("Sie muessen die LMRegist.calcTrafos implementieren, \n" +
                       "sonst wird nur eine dummy-Implemtierung verwendet, " +
                       "die falsche Ergebnisse liefert!");
    
    Vect3D fixed0  = new Vect3D(255.0610, 272.1390, 202.870); 
    Vect3D fixed1  = new Vect3D(208.9190, 312.8810, 87.1480);
    Vect3D fixed2  = new Vect3D(309.0100, 357.0810, 93.7060);
    Vect3D float0  = new Vect3D(-178.880, -64.770, 1659.270);
    Vect3D float1  = new Vect3D(-65.590, -95.910, 1712.930);
    Vect3D float2  = new Vect3D(-92.740, -5.130, 1760.030);
    /* Ausgabe in MITK FRE is 1.777 mm (Std.Dev. 0.480
     * min. Registration error (best fitting landmark) is 1.319 mm
     * max. Registration error (worst fitting landmark) is 2.275 mm
     */
    LMRegist.LMPair [] lms = new LMRegist.LMPair[3];
    lms[0] = new LMRegist.LMPair(fixed0, float0);
    lms[1] = new LMRegist.LMPair(fixed1, float1);
    lms[2] = new LMRegist.LMPair(fixed2, float2);
    
    ArrayList<Matrix3D> trafos = LMRegist.calcTrafos(lms);
    
    double   minFRE      = Double.MAX_VALUE;
    
    Matrix3D minFRETrafo = new Matrix3D(0);
    for(int i = 0; i < trafos.size(); i++)
     {
      System.out.println("FRE Trafo[" + i + "]:");
      double fre = LMRegist.calcFRE(lms, trafos.get(i));
      if(fre < minFRE) 
       {
        minFRE = fre;
        minFRETrafo = trafos.get(i);        
       }
     }
     
    System.out.printf("MinFRE: %7.2f\n", minFRE);
    minFRETrafo.print("minFRETrafo");
    int print = 2;
    Vect3D [] points = new Vect3D[3];
    points[0] = float0;
    points[1] = float1;
    points[2] = float2;
    System.out.printf("Punkte\n");
    for(int i = 0; i < points.length; i++)
     {
      points[i].print("P[" + i + "]");
     }
    Vect3D target = Vect3D.mean(points);
    target.print("Angenommener TP (Schwerpunkt)");
    double tre = LMRegist.calcTRE(target, minFRE, points, minFRETrafo);
    System.out.printf("TRE:  %7.2f \n", tre);

    // TRE                    fuer Trafo     fuer Trafo
    //                       mit FRE 2.34   mit FRE 1.83 (kleinster FRE)
    // TRE fuer Zentrum (mean): 2.34,           1,83
    // TRE fuer p[0]:           3.03            2.36
    // TRE fuer [0,0,0]:      26,996           21.04  
    System.out.println("\n\n\n");
    
   }
  
  /**
   * Beispiel zur Berechung von FRE und TRE
   * Fuenf zufaellige Punkte 
   *   (10.0, 45.0, 0.0), (53.0, 119.0, 3.0), (337.0, 411.0, 0.0),
   *   (273.0, 369.0, 11.0), (219.0, 61.0, 23.0)
   * werden einer Transformation unterworfen
   *   (rx = 9�, ry = 11�, rz = 13�)
   * Die transformierten Punkte werden um zuf�llige Strecken verschoben.
   * Auf Basis der ersten drei Punke werden die Transformationen berechnet, 
   * mit denen die transformierten Koordinaten auf die urspruenglichen 
   * abgebildet werden koennen.
   * Ueber alle Transformationen wird der FRE berechnet.
   * Mit dem Transformation, welche den niedrigsten FRE liefert, wird der TRE
   * fuer einen Punkt weit ausserhalb der gegebenen Punkte (-100,-50,0) 
   * und fuer einen Punkt (100, 100, 0) im Bereich der Punkte 
   * berechnet
   */
  public void beisp4_TREBerechnung()
   {
    System.out.println("beisp4_TREBerechnung\n");
    System.out.println("Sie muessen die LMRegist.calcTrafos implementieren, \n" +
      "sonst wird nur eine dummy-Implemtierung verwendet, " +
      "die falsche Ergebnisse liefert!");
    
    Random    randomizer = new Random(42);
    double    rand  = randomizer.nextFloat();  
    Vect3D    t1    = new Vect3D(256, 256, 0);
    Matrix3D  t1Mat = Matrix3D.translation(t1);
//    Matrix3D  t1Mat = new Matrix3D(1); //.translation(t1);
    Vect3D    t2    = new Vect3D(-256, -256, 0);
    Matrix3D  t2Mat = new Matrix3D(1);
    Matrix3D  rXMat = Matrix3D.rotationX(Math.toRadians(9));
    Matrix3D  ryMat = Matrix3D.rotationY(Math.toRadians(11));
    Matrix3D  rzMat = Matrix3D.rotationZ(Math.toRadians(13));
    Matrix3D  trafo = new Matrix3D(t1Mat);
    trafo = trafo.mult(rXMat);
    trafo = trafo.mult(ryMat);
    trafo = trafo.mult(rzMat);
    trafo = trafo.mult(t2Mat);
    
    Vect3D p1a = new Vect3D(10.0, 45.0, 0.0);
    Vect3D p1b = trafo.mult(p1a);
    Vect3D p1b_exact = new Vect3D(p1b);
    rand  = randomizer.nextFloat();  
    p1b.x = p1b.x + 1.0 * rand;
    rand  = randomizer.nextFloat();  
    p1b.y = p1b.y + 1.0 * rand;
    rand  = randomizer.nextFloat();  
    p1b.z = p1b.z + 1.0 * rand;

    Vect3D p2a = new Vect3D(53.0, 119.0, 3.0);
    Vect3D p2b = trafo.mult(p2a);
    Vect3D p2b_exact = new Vect3D(p2b);
    rand  = randomizer.nextFloat();  
    p2b.x = p2b.x - 1.0 * rand;
    rand  = randomizer.nextFloat();  
    p2b.y = p2b.y - 1.0 * rand;
    rand  = randomizer.nextFloat();  
    p2b.z = p2b.z - 1.0 * rand;

    Vect3D p3a       = new Vect3D(337.0, 411.0, 0.0);
    Vect3D p3b       = trafo.mult(p3a);
    Vect3D p3b_exact = new Vect3D(p3b);
    rand  = randomizer.nextFloat();  
    p3b.x = p3b.x + 3.0 * rand;
    rand  = randomizer.nextFloat();  
    p3b.y = p3b.y + 3.0 * rand;
    rand  = randomizer.nextFloat();  
    p3b.z = p3b.z + 3.0 * rand;
    
    Vect3D p4a       = new Vect3D(273.0, 369.0, 11.0);
    Vect3D p4b       = trafo.mult(p4a);
    Vect3D p4b_exact = new Vect3D(p4b);
    rand  = randomizer.nextFloat();  
    p4b.x = p4b.x - 3 * rand;
    rand  = randomizer.nextFloat();  
    p4b.y = p4b.y - 3 * rand;
    rand  = randomizer.nextFloat();  
    p4b.z = p4b.z - 3 * rand;
    
    Vect3D p5a       = new Vect3D(219.0, 61.0, 23.0);
    Vect3D p5b       = trafo.mult(p5a);
    Vect3D p5b_exact = new Vect3D(p5b);
    rand  = randomizer.nextFloat();  
    p5b.x = p5b.x + 3 * rand;
    rand  = randomizer.nextFloat();  
    p5b.y = p5b.y + 3 * rand;
    rand  = randomizer.nextFloat();  
    p5b.z = p5b.z + 3 * rand;
    
    Vect3D.print("a    ", p1a, p2a, p3a);
    Vect3D.print("b-ex ", p1b_exact, p2b_exact, p3b_exact);
    Vect3D.print("b    ", p1b, p2b, p3b);
    
    LMRegist.LMPair [] lms = new LMRegist.LMPair[3];
    lms[0] = new LMRegist.LMPair(p1a, p1b);
    lms[1] = new LMRegist.LMPair(p2a, p2b);
    lms[2] = new LMRegist.LMPair(p3a, p3b);
    
    ArrayList<Matrix3D> trafos = LMRegist.calcTrafos(lms);
    
    double   minFRE      = Double.MAX_VALUE;
    
    Matrix3D minFRETrafo = new Matrix3D(0);
    for(int i = 0; i < trafos.size(); i++)
     {
      double fre = LMRegist.calcFRE(lms, trafos.get(i));
      if(fre < minFRE) 
       {
        minFRE = fre;
        minFRETrafo = trafos.get(i);        
       }
     }  
    System.out.printf("MinFRE: %7.2f\n", minFRE);
    minFRETrafo.print("minFRETrafo");
    
    Vect3D [] pointsB = new Vect3D [5];
    pointsB[0] = new Vect3D(p1b);
    pointsB[1] = new Vect3D(p2b);
    pointsB[2] = new Vect3D(p3b);
    pointsB[3] = new Vect3D(p4b);
    pointsB[4] = new Vect3D(p5b);
    
    Vect3D [] pointsA = new Vect3D [5];
    pointsA[0] = new Vect3D(p1a);
    pointsA[1] = new Vect3D(p2a);
    pointsA[2] = new Vect3D(p3a);
    pointsA[3] = new Vect3D(p4a);
    pointsA[4] = new Vect3D(p5a);
    System.out.printf("p1a.x %7.2f .y %7.2f .z %7.2f p1b.x %7.2f .y %7.2f .z %7.2f\n", 
                      p1a.x, p1a.y, p1a.z, p1b.x, p1b.y, p1b.z);
    System.out.printf("p2a.x %7.2f .y %7.2f .z %7.2f p2b.x %7.2f .y %7.2f .z %7.2f\n", 
                      p2a.x, p2a.y, p2a.z, p2b.x, p2b.y, p2b.z);
    System.out.printf("p3a.x %7.2f .y %7.2f .z %7.2f p3b.x %7.2f .y %7.2f .z %7.2f\n", 
                      p3a.x, p3a.y, p3a.z, p3b.x, p3b.y, p3b.z);
    System.out.printf("p4a.x %7.2f .y %7.2f .z %7.2f p4b.x %7.2f .y %7.2f .z %7.2f\n", 
                      p4a.x, p4a.y, p4a.z, p4b.x, p4b.y, p4b.z);
    System.out.printf("p5a.x %7.2f .y %7.2f .z %7.2f p5b.x %7.2f .y %7.2f .z %7.2f\n", 
                      p5a.x, p5a.y, p5a.z, p5b.x, p5b.y, p5b.z);
/*    
    p1a.x   10,00 .y   45,00 .z    0,00 p1b.x  196,35 .y  351,12 .z   -4,49
    p2a.x   53,00 .y  119,00 .z    3,00 p2b.x  223,48 .y  430,37 .z    1,26
    p3a.x  337,00 .y  411,00 .z    0,00 p3b.x  440,51 .y  779,32 .z  -10,45
    p4a.x  273,00 .y  369,00 .z   11,00 p4b.x  384,98 .y  719,93 .z    4,60
    p5a.x  219,00 .y   61,00 .z   23,00 p5b.x  399,29 .y  410,39 .z  -17,58
*/    
    // targetb sei target in Koordinatensystem B (floating)
    // 1. Zielpunkt weit ausserhalb
    Vect3D targetB = new Vect3D(-100,-50,0);
    Vect3D targetA = minFRETrafo.mult(targetB);
    System.out.printf("targetA.x %7.2f .y %7.2f .z %7.2f  ",
                      targetA.x, targetA.y, targetA.z);
    System.out.printf("targetB.x %7.2f .y %7.2f .z %7.2f\n",
                      targetB.x, targetB.y, targetB.z);
    LMRegist.calcTRE(targetB, minFRE, pointsB, minFRETrafo);
    
    System.out.println("\n");
    // 2. Zielpunkt in der Naehe des Zentrums
    targetB = new Vect3D(300,500,0);
    targetA = minFRETrafo.mult(targetB);
    LMRegist.calcTRE(targetB, minFRE, pointsB, minFRETrafo);
    System.out.println("\n\n\n");
   }
  
 }
